#include "cv_lidar.hpp"

#include <cmath>

#include "spdlog/spdlog.h"

namespace {

const int kATT_GAIN = 5;
const int kREP_GAIN = 100000;

const int kGRID = 20;

// const int kSAFE = 300;                         // [pixel]  <=>   3  [m]
const int kSAFE = 150;                         // [pixel]  <=>   1  [m]
const int kLENGTH = 500;                       // [pixel]  <=>   10 [m]
constexpr int kCENTER = kLENGTH / 2;           // [pixel]
constexpr int kLENGTH_GRID = kLENGTH / kGRID;  // [grid]
constexpr int kCENTER_GRID = kCENTER / kGRID;  // [grid]
constexpr int kSAFE_GRID = kSAFE / kGRID;      // [grid]

const std::vector<component::Nmea> kMAP_PTS = {};

const std::vector<cv::Point2i> kMENTION = {
    cv::Point2i(kCENTER_GRID - kSAFE_GRID, kCENTER_GRID - kSAFE_GRID),
    cv::Point2i(kCENTER_GRID, kCENTER_GRID - kSAFE_GRID),
    cv::Point2i(kCENTER_GRID + kSAFE_GRID, kCENTER_GRID - kSAFE_GRID),
    cv::Point2i(kCENTER_GRID - kSAFE_GRID, kCENTER_GRID),
    cv::Point2i(kCENTER_GRID + kSAFE_GRID, kCENTER_GRID),
    cv::Point2i(kCENTER_GRID - kSAFE_GRID, kCENTER_GRID + kSAFE_GRID),
    cv::Point2i(kCENTER_GRID, kCENTER_GRID + kSAFE_GRID),
    cv::Point2i(kCENTER_GRID + kSAFE_GRID, kCENTER_GRID + kSAFE_GRID),
};

// [0] [1] [2]
// [3]     [4]
// [5] [6] [7]

}  // namespace

component::LidarPoint LidarField::FindDrivingPoint(
    const component::Nmea &coordinate) {
  (void)coordinate;
  component::LidarPoint pt;
  // TODO
  return pt;
}

double LidarField::CalcAttPot(const cv::Point2i &cur) {
  return 0.5 * kATT_GAIN * component::Distance(cur, drive_);
}

double LidarField::CalcRepPot(const cv::Point2i &cur, const double &radius) {
  double repulsive = 0;
  auto min_dis = component::Distance(
      *std::min_element(obstracts_.begin(), obstracts_.end(),
                        [&](cv::Point a, cv::Point b) {
                          return component::Distance(a, cur) <
                                 component::Distance(b, cur);
                        }),
      cur);
  if (min_dis <= radius / kGRID) {
    if (min_dis <= kSAFE_GRID) min_dis = kSAFE_GRID;
    repulsive =
        0.5 * kREP_GAIN * std::pow((1 / min_dis - 1 / (radius / kGRID)), 2);
  }
  return repulsive;
}

LidarField::LidarField(ScanMode mode) {
  mode_ = mode;
  map_ = cv::Mat(kLENGTH_GRID, kLENGTH_GRID, CV_8UC3, painting::kBLACK);
  SPDLOG_TRACE("Constructed");
}

LidarField::~LidarField() { SPDLOG_TRACE("Destructed"); }

void LidarField::ChangeMode(ScanMode mode) { mode_ = mode; }

void LidarField::SetDrivePoint(const component::Nmea &coordinate) {
  drive_ = component::Convert(FindDrivingPoint(coordinate), kCENTER) / kGRID;
}

void LidarField::SetDrivePoint(const component::LidarPoint &coordinate) {
  drive_ = component::Convert(coordinate, kCENTER) / kGRID;
}

void LidarField::SetObstracts(
    const std::vector<component::LidarPoint> &points) {
  SPDLOG_INFO("Points Size : {}", points.size());

  obstracts_.clear();
  if (points.size() > 2)
    for (auto &pt : points) {
      if (pt.dist_ > 500) continue;
      if (int(pt.dist_) != 0)
        obstracts_.emplace_back(component::Convert(pt, kCENTER) / kGRID);
    }
  std::sort(obstracts_.begin(), obstracts_.end(),
            [](cv::Point2i a, cv::Point2i b) {
              return (a.x == b.x) ? a.y < b.y : a.x < b.x;
            });
  obstracts_.erase(std::unique(obstracts_.begin(), obstracts_.end()),
                   obstracts_.end());
  SPDLOG_INFO("Size : {}", obstracts_.size());
}

void LidarField::CalPotField(double radius) {
  // SPDLOG_WARN("{} {} {} {}", min_x, max_x, min_y, max_y);
  for (int i = 0; i < kLENGTH_GRID; i++)
    for (int j = 0; j < kLENGTH_GRID; j++) {
      auto ug = CalcAttPot(cv::Point2i(i, j));
      auto uo = CalcRepPot(cv::Point2i(i, j), radius);
      auto uf = ug + uo;
      // SPDLOG_WARN("({}, {}) {} + {} = {}", i, j, ug, uo, uf);
      map_.at<cv::Vec3b>(i, j)[0] = int(uf);
    }
}

component::Velocity LidarField::CallDirection(double radius) {
  component::Velocity v;

  if (obstracts_.empty()) {
    SPDLOG_WARN("Obstracts empty");
    return v;
  }

  else
    CalPotField(radius);

  auto min = map_.at<cv::Vec3b>(kMENTION[0].x, kMENTION[0].y)[0];
  auto id = 0;

  for (std::size_t i = 1; i < 8; i++)
    if (min > map_.at<cv::Vec3b>(kMENTION[1].x, kMENTION[1].y)[0]) id = i;

  if (id == 0 || id == 3 || id == 5)
    v.wz = -1;
  else if (id == 2 || id == 4 || id == 7)
    v.wz = 1;

  if (id < 3)
    v.vx = 1;
  else if (id > 4)
    v.vx = -1;

  return v;
}

cv::Mat LidarField::VisualizeObstracts(int factor) {
  cv::Mat result;
  cv::resize(map_, result,
             cv::Size(factor * kLENGTH_GRID, factor * kLENGTH_GRID));

  if (!obstracts_.empty()) {
    cv::circle(result,
               cv::Point2i(kCENTER_GRID * factor, kCENTER_GRID * factor),
               kSAFE_GRID * factor, painting::kRED, 1);
    for (auto &pt : obstracts_)
      cv::circle(result, cv::Point2d(pt.x * factor, pt.y * factor), 1,
                 painting::kRED);
  }
  return result;
}

cv::Mat VisualizeMeasure(cv::Mat output, std::vector<float> pts) {
  cv::Mat result = output;
  auto cal = [&](float angle, float dist) {
    auto theta = (angle - 180) / 180.0 * M_PI;
    return cv::Point2i(static_cast<int>(std::round(200 + dist * sin(theta))),
                       static_cast<int>(std::round(200 + dist * cos(theta))));
  };
  if (pts.size() > 0)
    for (std::size_t i = 0; i < pts.size(); i++) {
      auto pt = cal(i * 0.5, pts[i]);
      cv::circle(result, cv::Point2d(pt.x, pt.y), 1, painting::kRED);
    }

  return result;
}

cv::Mat VisualInner(cv::Mat output,
                         std::vector<component::LidarPoint> pts) {
  cv::Mat result = output;
  auto cal = [&](float angle, float dist) {
    auto theta = (angle - 180) / 180 * M_PI;
    return cv::Point2i(static_cast<int>(std::round(200 + dist * sin(theta))),
                       static_cast<int>(std::round(200 + dist * cos(theta))));
  };
  if (pts.size() > 0)
    for (std::size_t i = 0; i < pts.size(); i++) {
      auto pt = cal(pts[i].angle_, pts[i].dist_);
      cv::circle(result, cv::Point2d(pt.x, pt.y), 1, painting::kBLUE);
    }

  return result;
}
